#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <vector>
#include <cmath>
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <termios.h>
#include <unistd.h>

#define M_PI 3.14159265358979323846

using namespace std::chrono_literals;


// 笛卡尔路径规划辅助函数
bool move_l(
    moveit::planning_interface::MoveGroupInterface& move_group,
    geometry_msgs::msg::Pose target_pose,
    double step_size = 0.01,
    double jump_threshold = 0.0)
  {
    std::vector<geometry_msgs::msg::Pose> waypoints;
    waypoints.push_back(target_pose);
  
    moveit_msgs::msg::RobotTrajectory trajectory;
    const double fraction = move_group.computeCartesianPath(
        waypoints,   // 路径点集合
        step_size,   // 笛卡尔路径步长（米）
        jump_threshold,  // 跳跃阈值（设为0禁用）
        trajectory
    );
  
    if (fraction >= 0.9) { // 允许部分成功
        moveit::planning_interface::MoveGroupInterface::Plan plan;
        plan.trajectory_ = trajectory;
        move_group.execute(plan);
        return true;
    } else {
        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), 
            "笛卡尔路径规划失败 (完成度: %.2f%%)", fraction * 100.0);
        return false;
    }
}


// 非阻塞键盘输入检测
int getch()
{
    static struct termios oldt, newt;
    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;
    newt.c_lflag &= ~(ICANON);
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
    int c = getchar();
    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    return c;
}

// 生成标定位姿序列
#include <cmath>
#include <vector>
#include <geometry_msgs/msg/pose.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

std::vector<geometry_msgs::msg::Pose> generate_calibration_poses(
    geometry_msgs::msg::Pose start_pose)
{
    std::vector<geometry_msgs::msg::Pose> poses;
    const double pos_step = 0.05;  // 减小平移步长至5cm
    const double ang_step = M_PI/9; // 角度步长20度

    // 获取初始姿态参数
    tf2::Quaternion q_initial(start_pose.orientation.x, start_pose.orientation.y,
                            start_pose.orientation.z, start_pose.orientation.w);
    tf2::Matrix3x3 m_initial(q_initial);
    double roll, pitch, yaw;
    m_initial.getRPY(roll, pitch, yaw);
    //start_pose.position.y += 0.05; // 初始位置偏移5cm
    // 主基准点（初始位置）
    poses.push_back(start_pose); // 基准点0

    // 生成平移基准点（XYZ轴平移）
    std::vector<geometry_msgs::msg::Pose> base_poses;
    for(int axis = 0; axis < 3; ++axis) {
        geometry_msgs::msg::Pose p_plus = start_pose;
        geometry_msgs::msg::Pose p_minus = start_pose;
        
        if(axis == 0) { // X轴
            p_plus.position.x += pos_step;
            p_minus.position.x -= pos_step;
        } else if(axis == 1) { // Y轴
            p_plus.position.y += pos_step;
            p_minus.position.y -= pos_step;
        } else { // Z轴
            p_plus.position.z += pos_step;
            p_minus.position.z -= pos_step;
        }
        base_poses.push_back(p_plus);
        base_poses.push_back(p_minus);
    }

    // 为每个基准点生成姿态变化
    for(auto& base_pose : base_poses) {
        // 保持位置不变，生成旋转变化
        for(int rot_axis = 0; rot_axis < 3; ++rot_axis) {
            geometry_msgs::msg::Pose p = base_pose;
            tf2::Quaternion q(base_pose.orientation.x, base_pose.orientation.y,
                             base_pose.orientation.z, base_pose.orientation.w);
            tf2::Matrix3x3 m(q);
            double r, p_ang, y;
            m.getRPY(r, p_ang, y);

            // 生成正负角度变化
            for(int sign = -1; sign <= 1; sign += 2) {
                if(rot_axis == 0) { // Roll
                    q.setRPY(r + sign*ang_step, p_ang, y);
                } else if(rot_axis == 1) { // Pitch
                    q.setRPY(r, p_ang + sign*ang_step, y);
                } else { // Yaw
                    q.setRPY(r, p_ang, y + sign*ang_step);
                }
                q.normalize();
                p.orientation = tf2::toMsg(q);
                poses.push_back(p);
            }
        }
    }

    // 最终姿态数量控制（17个）
    //if(poses.size() > 17) {
        // 优先保留组合运动位姿
    //    poses.resize(17); 
    //} else if(poses.size() < 17) {
        // 补充综合旋转
    //    for(int i = 0; poses.size() < 17; ++i) {
    //        geometry_msgs::msg::Pose p = start_pose;
    //        tf2::Quaternion q;
     //       q.setRPY(roll + i*0.1, pitch - i*0.1, yaw + i*0.1);
     //       p.orientation = tf2::toMsg(q);
     //       poses.push_back(p);
     //   }
    //}

    return poses;
}

int main(int argc, char** argv)
{
    // Initialize ROS and create the Node
    rclcpp::init(argc, argv);
    auto const node = std::make_shared<rclcpp::Node>(
        "moveit_eyetohand_node",
        rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
    );

    // Create a ROS logger
    auto const logger = rclcpp::get_logger("moveit_eyetohand_node");

    // We spin up a SingleThreadedExecutor so MoveItVisualTools interact with ROS
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    auto spinner = std::thread([&executor]() { executor.spin(); });

    // Create the MoveIt MoveGroup Interface
    using moveit::planning_interface::MoveGroupInterface;
    auto move_group = MoveGroupInterface(node, "ur_manipulator");

    auto tf_buffer = tf2_ros::Buffer(node->get_clock());
    auto tf_listener = tf2_ros::TransformListener(tf_buffer, node);


    // 获取xarm规划组的规划参考坐标系
    std::string planning_frame = move_group.getPlanningFrame();
    RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), "Planning frame : "<< planning_frame);
    // 获取末端执行器的link
    std::string eef_link = move_group.getEndEffectorLink();
    RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), "End effector link : "<< eef_link);

    // 若allow_replanning()参数为True，则MoveIt!在一次规划失败后进行重新规划
    move_group.allowReplanning(true);
    // 设置运动到目标时的位置(单位：米)和姿态的容忍误差(单位：弧度)
    move_group.setGoalPositionTolerance(0.02);
    move_group.setGoalOrientationTolerance(0.03);
    // 设置一个比例因子以选择性地降低最大关节速度限制，可取值为(0,1]
    move_group.setMaxVelocityScalingFactor(0.5);

    // 获取初始位姿
    geometry_msgs::msg::TransformStamped transform;
    bool transform_received = false;

    while (!transform_received)
    {
        try
        {
            rclcpp::Time now = node->get_clock()->now();
            transform = tf_buffer.lookupTransform("base_link", "tool0", tf2::TimePointZero);
            transform_received = true; // 如果lookupTransform成功，设置为true以退出循环
        }
        catch (tf2::TransformException &ex)
        {
            RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "没有找到机器人末端夹爪当前位置, TF lookup failed: %s", ex.what());
            // 可以在这里添加一些等待时间，避免频繁请求
            std::this_thread::sleep_for(std::chrono::seconds(1));
        }
    }

    RCLCPP_INFO_STREAM(logger, "tool0 Transform: " << transform.transform.translation.x << ", "
                                                   << transform.transform.translation.y << ", "
                                                   << transform.transform.translation.z);
    geometry_msgs::msg::Pose start_pose;
    start_pose.position.x = transform.transform.translation.x;
    start_pose.position.y = transform.transform.translation.y;
    start_pose.position.z = transform.transform.translation.z;
    start_pose.orientation = transform.transform.rotation;

    start_pose = move_group.getCurrentPose().pose;   // 初始位姿为当前位姿

    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Robot pose x : %.2fm; y : %.2fm; z : %.2fm", 
    start_pose.position.x, start_pose.position.y, start_pose.position.z);

    // 生成标定位姿序列
    auto calibration_poses = generate_calibration_poses(start_pose);
    if(calibration_poses.size() < 17){
        RCLCPP_ERROR(node->get_logger(), "生成的标定位姿数量不足!");
        return 1;
    }

    RCLCPP_INFO(node->get_logger(), "标定程序准备就绪，共需移动 %zu 个位姿", calibration_poses.size());
    RCLCPP_INFO(node->get_logger(), "按Enter键开始标定流程...");
    getch(); // 等待首次按键

    // 执行标定位姿移动
    for(size_t i = 0; i < calibration_poses.size() && rclcpp::ok(); ++i){
        RCLCPP_INFO(node->get_logger(), "\n=== 正在移动到第 %zu/%zu 个标定位姿 ===", 
                   i+1, calibration_poses.size());
        
        if (!move_l(move_group, calibration_poses[i])) {
            RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "直线运动失败，终止程序");
            continue;
        }

        // 等待用户确认
        if(i != calibration_poses.size()-1) {
            RCLCPP_INFO(node->get_logger(), "按Enter继续下一个位姿...");
            getch();
        }
    }

    RCLCPP_INFO(node->get_logger(), "标定流程完成! 按下Enter返回初始位置");
    getch();
    move_group.setNamedTarget("home");
    move_group.move();

    rclcpp::shutdown();
    spinner.join();
    return 0;
}
